#ifndef __KALMAN__
#define __KALMAN__
#include <Arduino.h>
#include "BasicLinearAlgebra.h"
using namespace BLA;
class KalmanFilter3D
{
    public:
    KalmanFilter3D(Matrix<3, 1> &x, Matrix<3, 3> &P, Matrix<3, 3> &Q, Matrix<3, 3> &R,Matrix<3, 3> &F, Matrix<3, 3> &H, Matrix<3, 3> &B);
    KalmanFilter3D();
    void setX(Matrix<3, 1> x);
    void setP(Matrix<3, 3> P);
    void setQ(Matrix<3, 3> Q);
    void setR(Matrix<3, 3> R);
    void setF(Matrix<3, 3> F);
    void setH(Matrix<3, 3> H);
    void setB(Matrix<3, 3> G);
    void setU(Matrix<3, 1> u);
    void update(Matrix<3, 1> z);
    void predict();
    Matrix<3, 1> getX();
    Matrix<3, 3> getP();
    Matrix<3, 1> x;
    Matrix<3, 3> K;
    private:
    
    Matrix<3, 1> u;
    Matrix<3, 1> z;
    Matrix<3, 3> P;
    Matrix<3, 3> Q;
    Matrix<3, 3> R;
    Matrix<3, 3> F;
    Matrix<3, 3> H;
    Matrix<3, 3> B;


};
#endif 
